/*************************************************************
 * @file app.c
 * @author MicroOrange
 * @brief 
 * @date 2023/2/19
 *************************************************************/
#include "sorg_utils.h"
#include "aim_dir.h"
#include "remote_ps2.h"
#include "log.h"
#include "aim_484.h"
#include "usart.h"
#include "mg_can.h"
#include "pid.h"
#include "math.h"



void app_init(){
    orange_log_init();
    PS2_SetInit();
    can_user_init(&hcan);
    HAL_GPIO_WritePin(GPIOC, GPIO_PIN_13, GPIO_PIN_RESET);
//    set_motor_voltage(0, 5, 5, 5, 5);
}

uint32_t temp_count=0;

void app_loop(){
    temp_count++;
    uint16_t angle_now = motor_info[0].rotor_angle;
    int16_t speed_now = motor_info[0].rotor_speed;
    LOGLN_INFO("app", "nowang:%d", angle_now);
    uint16_t kp = 5;
    uint16_t aid = 100;
    if (angle_now< 3000)
        set_motor_voltage(0, (3000-angle_now)*kp, 0, 0, 0);
    else if (angle_now > 5000)
        set_motor_voltage(0, (5000-angle_now)*kp, 0, 0, 0);
    else
        if (speed_now > 5)
            set_motor_voltage(0 ,aid, 0, 0, 0);
        else if (speed_now < -5)
            set_motor_voltage(0 ,-aid, 0, 0, 0);
        else
            set_motor_voltage(0 ,0, 0, 0, 0);

//    set_motor_voltage(0, 5000, 0, 0, 0);
    HAL_Delay(10);
}